//
// Created by Administrator on 2016/12/9.
//

#include "Cube.h"

using namespace std;
using namespace glm;

vector<glm::mat4> Cube::instanceTransforms;
btDynamicsWorld *Cube::dynamicsWorld = nullptr;

Cube::~Cube() {
    Cube::dynamicsWorld->removeCollisionObject(mRigidBody);
    delete (mRigidBody);
    mRigidBody = nullptr;
}

Cube::Cube(btDynamicsWorld *dynamicsWorld, float size, btCollisionShape *pShape, glm::mat4 &modelTrans,
           AXESPOS &axesPos)
        : mSize(size), mAxesPosOrigin(axesPos) {
    mPosOrigin = vec3(axesPos.X * size, axesPos.Y * size, axesPos.Z * size);
    mat4 cubeTrans = translate(modelTrans, mPosOrigin);
    instanceTransforms.push_back(cubeTrans);
    mIndex = instanceTransforms.size() - 1;

    if (!Cube::dynamicsWorld && dynamicsWorld) {
        Cube::dynamicsWorld = dynamicsWorld;
    }

    /*add bullet physics collision*/
    btTransform bulletTransform;
    bulletTransform.setFromOpenGLMatrix(value_ptr(cubeTrans));
    btDefaultMotionState motionstate = btDefaultMotionState(bulletTransform);
    // mass, in kg. 0 -> Static object, will never move.collision shape of body. local inertia
    btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(0, &motionstate, pShape, btVector3(0, 0, 0));
    mRigidBody = new btRigidBody(rigidBodyCI);

    mRigidBody->setUserPointer(this);

    Cube::dynamicsWorld->addRigidBody(mRigidBody);

    updateAxesPos(nullptr);
}

void Cube::updateAxesPos(glm::mat4 *rubikTrans) {
    vec3 position = instanceTransforms[mIndex][3];
    if (rubikTrans) {
        position = glm::inverse(*rubikTrans) * vec4(position, 1);
    }
    mAxesPos.X = round(position[0] / mSize);
    mAxesPos.Y = round(position[1] / mSize);
    mAxesPos.Z = round(position[2] / mSize);
}

void Cube::resetTransform(glm::mat4 transform, mat4 *rubikTrans) {
    instanceTransforms[mIndex] = transform;

    btTransform btTrans;
    btTrans.setFromOpenGLMatrix(value_ptr(transform));
    mRigidBody->setWorldTransform(btTrans);

    if (rubikTrans){
        updateAxesPos(rubikTrans);
    }
}
